#include "RigidBodyComp.h"
#include <SECore/Ecs/Actor.h>
#include <SEngine/Ecs/Components/TransformComp.h>
#define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/matrix_decompose.hpp>
#include <glm/gtx/euler_angles.hpp>
#include <SEPhysics/Entity/BoxShape.h>
#include <SEPhysics/Entity/SphereShape.h>
#include <SEMath/Util/Util.h>

#include <SEDebug/Log/Log.h>
#include <glm/gtx/string_cast.hpp>

namespace SEngine
{
    RigidBodyComp::RigidBodyComp(Actor &actor)
        : Component(actor)
    {
        m_enabled = false;
    }

    RigidBodyComp::~RigidBodyComp()
    {

    }

    void RigidBodyComp::BeforeUpdate()
    {

    }

    void RigidBodyComp::Attach()
    {
        m_actor.Subscribe<RigidBodyComp, glm::mat4&>("SignalUpdateTransform", [this](glm::mat4 & model) {
            lastState = glm::translate(model, offset);
            Transform(lastState);
            return SlotState::Keep;
        });
    }

    void RigidBodyComp::OnUpdate(int delta)
    {
        auto & comp = m_actor.GetComponent<TransformComp>()->get();
        lastState = glm::translate(comp.GetModel(), offset);
        Transform(lastState);
    }

    void RigidBodyComp::StartSimulation()
    {
        // auto & comp = m_actor.GetComponent<TransformComp>()->get();
        // initialState = comp.GetModel();
        initialState = lastState;
        lastState = glm::translate(initialState, offset);
        BodySetting setting {
            lastState,
            linearVelocity,
            linearFactor,
            angularVelocity,
            angularFactor,
            friction,
            restitution,
            mass
        };
        switch (shapeType)
        {
        case BaseShape::Box:    setting.shape = std::make_unique<BoxShape>(size); break;
        case BaseShape::Sphere: setting.shape = std::make_unique<SphereShape>(radius); break;
        default: break;
        }
        CreateBody(setting);
        m_enabled = true;
    }

    void RigidBodyComp::StopSimulation()
    {
        auto & comp = m_actor.GetComponent<TransformComp>()->get();
        m_enabled = false;
        comp.SetModel(initialState);
        DestroyBody();
    }

    void RigidBodyComp::OnUpdateTransform()
    {
        auto & comp = m_actor.GetComponent<TransformComp>()->get();
        Decomposer m(Transform());
        Decomposer lastM(lastState);
        auto dTranslate = m.translate - lastM.translate;
        auto dQuat = m.quat - lastM.quat;
        Decomposer curM(comp.GetModel());
        glm::mat4 targetM = glm::translate(glm::mat4{1.0f}, curM.translate + dTranslate) * glm::mat4_cast(curM.quat + dQuat) * glm::scale(glm::mat4{1.0f}, curM.scale);
        comp.SetModel(targetM);
        lastState = m.matrix;
    }

} // namespace SEngine

